/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "fc_sensors.h"
#include "filters.h"
#include "fc_state.h"
#include "icm42688p_read_polling.h"

#define DBG_TAG "gyro"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

biquadFilter_t gyro_lpf1[3];
biquadFilter_t gyro_lpf2[3];
extern float F1_wave[12];
bool gyro_lpf_init = false;
bool gyro_use_lpf = true;
/**
 * @brief 初始化陀螺仪低通滤波器
 *
 * @param cutoff 0: load from config else: set cutoff
 */
void sensor_gyro_init(uint16_t cutoff)
{
    if (cutoff != 0)
    {
        if (cutoff < 500)
        {
            imu_offset()->gyro_cutoff = cutoff;
            imu_offset()->flag = IMU_SAVED_FLAG;
            zino_config_save_all();
        }
    }
    else
    {
        if (imu_offset()->flag == IMU_SAVED_FLAG)
        {
            fc_sensor_data.gyro.gyro_offset.isdatacalibrated = imu_offset()->gyro_calibrated;
            fc_sensor_data.gyro.gyro_offset.zero.x = imu_offset()->gyrooffset.x;
            fc_sensor_data.gyro.gyro_offset.zero.y = imu_offset()->gyrooffset.y;
            fc_sensor_data.gyro.gyro_offset.zero.z = imu_offset()->gyrooffset.z;
            fc_sensor_data.gyro.gyro_offset.samplesize = 0;
            LOG_I("gyro offset: %d, %d, %d\n", fc_sensor_data.gyro.gyro_offset.zero.x, fc_sensor_data.gyro.gyro_offset.zero.y, fc_sensor_data.gyro.gyro_offset.zero.z);
        }
        else
        {
            imu_offset()->gyro_cutoff = GYRO_LPF_CUTOFF;
        }
        if (imu_offset()->gyro_cutoff >= 500 || imu_offset()->gyro_cutoff == 0)
        {
            imu_offset()->gyro_cutoff = GYRO_LPF_CUTOFF;
        }
        fc_sensor_data.gyro.scacle = get_gyro_scale_factor();
        LOG_I("gyro lpf cutoff: %dHz\n", imu_offset()->gyro_cutoff);
        rt_kprintf_float("scale: ", fc_sensor_data.gyro.scacle, " \n");
        for (int i = 0; i < 3; i++)
        {
            biquadFilterInitLPF(&gyro_lpf1[i], imu_offset()->gyro_cutoff, 1000.0f);
            // biquadFilterInitLPF(&gyro_lpf2[i], 50.0f, 1000.0f);
        }
    }
}
void GyroStartCalibration(void)
{
    fc_sensor_data.gyro.gyro_offset.isdatacalibrated = false;
    fc_sensor_data.gyro.gyro_offset.samplesize = ACC_GYRO_CALIBRATE_SAMPLE;
    FC_STATE_SET(FC_IMU_CALIBRATING);
}
void sensordata_save_gyro_bias(gyro_t *gyro)
{
    imu_offset()->gyrooffset.x = gyro->gyro_offset.zero.x;
    imu_offset()->gyrooffset.y = gyro->gyro_offset.zero.y;
    imu_offset()->gyrooffset.z = gyro->gyro_offset.zero.z;

    zino_config_save_all();
}
void sensordata_get_save_gyro_bias(gyro_t *gyro)
{
    gyro->gyro_offset.zero.x = imu_offset()->gyrooffset.x;
    gyro->gyro_offset.zero.y = imu_offset()->gyrooffset.y;
    gyro->gyro_offset.zero.z = imu_offset()->gyrooffset.z;
}
void sensordata_gyro_calibrate(gyro_t *gyro)
{
    if (gyro->gyro_offset.samplesize > 0)
    {
        if (gyro->gyro_offset.samplesize == ACC_GYRO_CALIBRATE_SAMPLE)
        {
            gyro->gyro_offset.samplesum.x = 0;
            gyro->gyro_offset.samplesum.y = 0;
            gyro->gyro_offset.samplesum.z = 0;
        }

        gyro->gyro_offset.samplesum.x += gyro->gyroRaw.x;
        gyro->gyro_offset.samplesum.y += gyro->gyroRaw.y;
        gyro->gyro_offset.samplesum.z += gyro->gyroRaw.z;

        if (gyro->gyro_offset.samplesize == 1)
        {
            gyro->gyro_offset.zero.x = gyro->gyro_offset.samplesum.x / ACC_GYRO_CALIBRATE_SAMPLE;
            gyro->gyro_offset.zero.y = gyro->gyro_offset.samplesum.y / ACC_GYRO_CALIBRATE_SAMPLE;
            gyro->gyro_offset.zero.z = gyro->gyro_offset.samplesum.z / ACC_GYRO_CALIBRATE_SAMPLE;
            gyro->gyro_offset.isdatacalibrated = true;

            gyro->scacle = get_gyro_scale_factor();

            imu_offset()->gyrooffset.x = gyro->gyro_offset.zero.x;
            imu_offset()->gyrooffset.y = gyro->gyro_offset.zero.y;
            imu_offset()->gyrooffset.z = gyro->gyro_offset.zero.z;
            imu_offset()->gyro_calibrated = true;
            imu_offset()->flag = IMU_SAVED_FLAG;
            zino_config_save_all();
            FC_STATE_CLEAR(FC_IMU_CALIBRATING);
        }
        gyro->gyro_offset.samplesize--;
        // rt_kprintf("gyro calibrate %d\r\n", gyro->gyro_offset.samplesize);
    }
}
void sensor_gyro_process(gyro_t *gyro)
{

    if (!imu_offset()->gyro_calibrated || !gyro->gyro_offset.isdatacalibrated)
    {
        if (!imu_offset()->gyro_calibrated)
        {
            imu_offset()->gyrooffset.x = 0;
            imu_offset()->gyrooffset.y = 0;
            imu_offset()->gyrooffset.z = 0;
        }
        if (gyro->gyro_offset.samplesize == 0)
        {
            GyroStartCalibration();
        }
        sensordata_gyro_calibrate(gyro);
    }

    // gyro->gyrodata.x = (float)(gyro->gyroRaw.x - gyro->gyro_offset.zero.x) * gyro->scacle;
    // gyro->gyrodata.y = (float)(gyro->gyroRaw.y - gyro->gyro_offset.zero.y) * gyro->scacle;
    // gyro->gyrodata.z = (float)(gyro->gyroRaw.z - gyro->gyro_offset.zero.z) * gyro->scacle;

    for (uint8_t i = 0; i < 3; i++)
    {
        gyro->gyrodata.axis[i] = (float)(gyro->gyroRaw.axis[i] - gyro->gyro_offset.zero.axis[i]) * gyro->scacle;
        if (gyro_use_lpf)
        {
            gyro->gyrolpf.axis[i] = biquadFilterApply(&gyro_lpf1[i], gyro->gyrodata.axis[i]);
        }
        else
        {
            gyro->gyrolpf.axis[i] = gyro->gyrodata.axis[i];
        }
        gyro->gyroSum.axis[i] += gyro->gyrolpf.axis[i];
        gyro->sumCount.axis[i]++;
    }
}
void gyro_update(void)
{
    // icm42688p_gyro_read_polling2(fc_sensor_data.gyro.gyroRaw.axis);
    // sensor_imc42688p_poll_data(&fc_sensor_data);
    sensor_gyro_process(&fc_sensor_data.gyro);
}
